/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file    fdcan.c
  * @brief   This file provides code for the configuration
  *          of the FDCAN instances.
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "fdcan.h"

/* USER CODE BEGIN 0 */
#include "string.h"
#include "service.h"
#include "motor.h"
#include "my_can.h"
/* USER CODE END 0 */

FDCAN_HandleTypeDef hfdcan1;

/* FDCAN1 init function */
void MX_FDCAN1_Init(void)
{

  /* USER CODE BEGIN FDCAN1_Init 0 */

  /* USER CODE END FDCAN1_Init 0 */

  /* USER CODE BEGIN FDCAN1_Init 1 */

  /* USER CODE END FDCAN1_Init 1 */
  hfdcan1.Instance = FDCAN1;
  hfdcan1.Init.ClockDivider = FDCAN_CLOCK_DIV1;
  hfdcan1.Init.FrameFormat = FDCAN_FRAME_FD_BRS;
  hfdcan1.Init.Mode = FDCAN_MODE_NORMAL;
  hfdcan1.Init.AutoRetransmission = DISABLE;
  hfdcan1.Init.TransmitPause = DISABLE;
  hfdcan1.Init.ProtocolException = DISABLE;
  hfdcan1.Init.NominalPrescaler = 17;
  hfdcan1.Init.NominalSyncJumpWidth = 2;
  hfdcan1.Init.NominalTimeSeg1 = 7;
  hfdcan1.Init.NominalTimeSeg2 = 2;
  hfdcan1.Init.DataPrescaler = 2;
  hfdcan1.Init.DataSyncJumpWidth = 2;
  hfdcan1.Init.DataTimeSeg1 = 13;
  hfdcan1.Init.DataTimeSeg2 = 3;
  hfdcan1.Init.StdFiltersNbr = 1;
  hfdcan1.Init.ExtFiltersNbr = 1;
  hfdcan1.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION;
  if (HAL_FDCAN_Init(&hfdcan1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN FDCAN1_Init 2 */
  // 配置接收所有标准 ID 的过滤器
  FDCAN_FilterTypeDef sFilterConfig;
  sFilterConfig.IdType = FDCAN_STANDARD_ID;
  sFilterConfig.FilterIndex = 0;
  sFilterConfig.FilterType = FDCAN_FILTER_MASK;
  sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
  sFilterConfig.FilterID1 = 0x001; // 过滤器ID
  sFilterConfig.FilterID2 = 0x7FF; // 过滤器掩码，0x000表示接收所有ID
  if (HAL_FDCAN_ConfigFilter(&hfdcan1, &sFilterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_FDCAN_ConfigGlobalFilter(&hfdcan1, FDCAN_REJECT, FDCAN_REJECT, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE) != HAL_OK)
  {
    Error_Handler();
  }
  // 激活通知
  if (HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0) != HAL_OK)
  {
    Error_Handler();
  }
  // 启动FDCAN模块
  if (HAL_FDCAN_Start(&hfdcan1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE END FDCAN1_Init 2 */

}

void HAL_FDCAN_MspInit(FDCAN_HandleTypeDef* fdcanHandle)
{

  GPIO_InitTypeDef GPIO_InitStruct = {0};
  RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};
  if(fdcanHandle->Instance==FDCAN1)
  {
  /* USER CODE BEGIN FDCAN1_MspInit 0 */

  /* USER CODE END FDCAN1_MspInit 0 */

  /** Initializes the peripherals clocks
  */
    PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_FDCAN;
    PeriphClkInit.FdcanClockSelection = RCC_FDCANCLKSOURCE_PCLK1;
    if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
    {
      Error_Handler();
    }

    /* FDCAN1 clock enable */
    __HAL_RCC_FDCAN_CLK_ENABLE();

    __HAL_RCC_GPIOA_CLK_ENABLE();
    /**FDCAN1 GPIO Configuration
    PA11     ------> FDCAN1_RX
    PA12     ------> FDCAN1_TX
    */
    GPIO_InitStruct.Pin = GPIO_PIN_11|GPIO_PIN_12;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    GPIO_InitStruct.Alternate = GPIO_AF9_FDCAN1;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

    /* FDCAN1 interrupt Init */
    HAL_NVIC_SetPriority(FDCAN1_IT0_IRQn, 5, 0);
    HAL_NVIC_EnableIRQ(FDCAN1_IT0_IRQn);
    HAL_NVIC_SetPriority(FDCAN1_IT1_IRQn, 5, 0);
    HAL_NVIC_EnableIRQ(FDCAN1_IT1_IRQn);
  /* USER CODE BEGIN FDCAN1_MspInit 1 */

  /* USER CODE END FDCAN1_MspInit 1 */
  }
}

void HAL_FDCAN_MspDeInit(FDCAN_HandleTypeDef* fdcanHandle)
{

  if(fdcanHandle->Instance==FDCAN1)
  {
  /* USER CODE BEGIN FDCAN1_MspDeInit 0 */

  /* USER CODE END FDCAN1_MspDeInit 0 */
    /* Peripheral clock disable */
    __HAL_RCC_FDCAN_CLK_DISABLE();

    /**FDCAN1 GPIO Configuration
    PA11     ------> FDCAN1_RX
    PA12     ------> FDCAN1_TX
    */
    HAL_GPIO_DeInit(GPIOA, GPIO_PIN_11|GPIO_PIN_12);

    /* FDCAN1 interrupt Deinit */
    HAL_NVIC_DisableIRQ(FDCAN1_IT0_IRQn);
    HAL_NVIC_DisableIRQ(FDCAN1_IT1_IRQn);
  /* USER CODE BEGIN FDCAN1_MspDeInit 1 */

  /* USER CODE END FDCAN1_MspDeInit 1 */
  }
}

/* USER CODE BEGIN 1 */
void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs)
{
    if ((RxFifo0ITs & FDCAN_IT_RX_FIFO0_NEW_MESSAGE) != RESET)
    {
        FDCAN_ReceiveData();
    }
}

// 接收数据示例
void FDCAN_ReceiveData(void)
{
  // FDCAN_RxHeaderTypeDef RxHeader;
  // uint8_t RxData[64]; // 最大数据长度为64字节

  FDCAN_MessageTypeDef message;

  if (HAL_FDCAN_GetRxMessage(&hfdcan1, FDCAN_RX_FIFO0, &message.RxHeader, message.RxData) == HAL_OK)
  {
    printf("Received message ID: 0x%03X\n", message.RxHeader.Identifier);
    printf("Received data: ");
    for (int i = 0; i < 8; i++)
    {
        printf("0x%02X ", message.RxData[i]);
    }
    printf("\n");
    switch (message.RxHeader.Identifier)
    {
    case MASTER_ID:
      canReceiveCounter++;
      
      process_can_message(&message);
      // if (LOCAL_ID == MOTOR0_ID)
      // {
      //   memcpy(&position_target, RxData, sizeof(float));
      // } else if (LOCAL_ID == MOTOR1_ID)
      // {
      //   memcpy(&position_target, RxData + 4, sizeof(float));
      // } else if (LOCAL_ID == MOTOR2_ID)
      // {
      //   memcpy(&position_target, RxData + 8, sizeof(float));
      // }
      // CANMessageData messageData;
      // memcpy(&messageData, RxData, 32);
      // global_mode = messageData.motorModes;

      // motor_target = messageData.motorTargets[0];
      // if (messageData.motorModes == MOTOR_MODE_POSITION)
      // {
      //   // motor_target.user_target.position = motor_target;
      //   // position_target = motor_target;
      // }
      // motor_target.
      // position_target = ;
      // Process the received message
      // printf("commandType%d\n", messageData.commandType);
      // printf("motor1: %f\n", messageData.motorTargets[0]);
      // printf("motor2: %f\n", messageData.motorTargets[1]);
      // printf("motor3: %f\n", messageData.motorTargets[2]);
      // printf("motor4: %f\n", messageData.motorTargets[3]);
      // printf("motor5: %f\n", messageData.motorTargets[4]);
      // printf("motor6: %f\n", messageData.motorTargets[5]);
      // printf("motor7: %f\n", messageData.motorTargets[6]);
      break;
    
    default:
      break;
    }
  }
  else
  {
      // 处理接收错误
      Error_Handler();
  }
}

// 发送数据示例
void FDCAN_SendData(uint32_t id, uint8_t *data, uint8_t length)
{
    FDCAN_TxHeaderTypeDef TxHeader;
    TxHeader.Identifier = id;
    TxHeader.IdType = FDCAN_STANDARD_ID; // 使用标准ID
    TxHeader.TxFrameType = FDCAN_DATA_FRAME;
    TxHeader.DataLength = length; // 数据长度，单位为字节
    TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
    TxHeader.BitRateSwitch = FDCAN_BRS_ON;
    TxHeader.FDFormat = FDCAN_FD_CAN;
    TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
    TxHeader.MessageMarker = 0;
    if ((hfdcan1.Instance->TXFQS & FDCAN_TXFQS_TFQF) != 0)
    {
      // 发送队列已满，复位 FDCAN 模块
      LED8_ON();
      printf("Transmit FIFO is full, resetting FDCAN...\n");

      // 停止 FDCAN 模块
      if (HAL_FDCAN_Stop(&hfdcan1) != HAL_OK)
      {
        printf("Error stopping FDCAN\n");
        Error_Handler();
      }

      // 重新初始化 FDCAN 模块
      MX_FDCAN1_Init();

      // 再次尝试发送数据
      if ((hfdcan1.Instance->TXFQS & FDCAN_TXFQS_TFQF) != 0)
      {
        printf("Transmit FIFO is still full after reset\n");
        return;
      }
      else
      {
        printf("FDCAN reset successful, retrying transmission\n");
      }
    } else {
      LED8_OFF();
    }
    if (HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &TxHeader, data) != HAL_OK)
    {
      // 处理发送错误
      LED8_ON();
      printf("Error sending message\n");
      Error_Handler();
    }
}
/* USER CODE END 1 */
